//
// Created by ze on 2023/11/2.
//

#ifndef ENCODER_UPRE_INT_H
#define ENCODER_UPRE_INT_H
#include "gpio.h"
#include "usart.h"
#include "math.h"
#include "stdio.h"
#include "stdint.h"
#include "stdbool.h"
#include "string.h"
#include "stdlib.h"//要使用malloc是要包含此头文件
#include "memory.h"//要使用memset是要包含此头文件
#include "stm32h7xx_hal.h"
#define PI 3.141593f

//#define BlueTeeth
#define MODE 0  //0:舵轮  1:全向轮  2:麦轮
/******************陀螺仪选择*************************/
#define GYRO_UART huart6
//#define GYRO_MINS500
#define GYRO_100A
//#define GYRO_GI410
/******************打印开关*********************/
#define Printf_Pose         1      //打印机器人当前位姿
#define Printf_Gyro         0      //打印陀螺仪数据
#define Printf_TargetVel    0       //打印路径参数
#define ANGLE_INIT_F   (0) //逆
#define ANGLE_INIT_L   (0)
#define ANGLE_INIT_R   (0)
#define STEERING_LOCK 1         //1为锁定轮电机与舵电机，0为解锁
#define LUN_MOTOR_LOCK_OFF  0   //0为舵电机归位到路径方向时才锁定轮电机，避免起步翘头  1为解锁
/******************串口接收*************************/

//机器人位姿
typedef struct _Pose {
    float wx;
    float wy;
    float yaw;
    float pitch;
    float roll;
} Pose;

//串口7接受标志位
typedef struct {
    uint8_t ValidBall; //1 valid 0 invalid
    uint8_t Start;
    uint8_t Wall;
    uint8_t Side;
    uint8_t Claw;
    uint8_t EnterDelay;
} SetSerialFlag;

//R2上层电机驱动
typedef struct {
    int32_t Guide;
    int32_t Harvest_L;
    int32_t Harvest_R;
    int32_t Divide_L;
    int32_t Divide_R;
    float Transport;
    //    float Claw;
} SetCollectMotor;

typedef struct _Point {
    float x;
    float y;
} point;

typedef struct _vector {
    float size;
    float dir;
} vector;

typedef struct _WheelParam {
    const float RotationRadius; //轮子半径
    const float PositiveDir; //自旋时轮子的固定角度
    float currentPos; //舵电机转过的总角度
    vector targetVel;
} WheelParam;

typedef struct _WheelSys {
    WheelParam LeftFront;
    WheelParam RightFront;
    WheelParam RightRear;
    WheelParam LeftRear;
} WheelSys;

typedef struct _motor_measure_t {
    float dir;
    float last_dir;
    float total_dir; //总转过的角度
} motor_measure;

typedef struct _WHEEL_Measure {
    motor_measure Wheel1_Measure;
    motor_measure Wheel2_Measure;
    motor_measure Wheel3_Measure;
    motor_measure Wheel4_Measure;
} Wheel_Name_Measure;

typedef struct _ChassisMovement {
    float omega;
    vector transVel;
} ChassisMovement;

typedef union Data16_ {
    uint8_t uint8_2[2];
    int16_t int16_;
    uint16_t uint16_;
} Data16;

typedef union Data32_ {
    uint8_t uint8_4[4];
    int16_t int16_2[2];
    uint16_t uint16_2[2];
    int32_t int32_;
    uint32_t uint32_;
    float float_;
} Data32;

typedef union Data64_ {
    uint8_t uint8_8[8];
    int16_t int16_4[4];
    uint16_t uint16_4[4];
    int32_t int32_2[2];
    uint32_t uint32_2[2];
    int64_t int64_;
    uint64_t uint64_;
    double double_;
} Data64;

/***************************************全场定位******************************************/


/****************************************路径*********************************************/
typedef enum {
    FRONT = 1,
    LEFT,
    RIGHT
} MotorOrder;

// 全场路径状态标志位
typedef enum PATH_STATE {
    GOING = 20,
    FINISH,
    NEW_PATH,
    PATH_ERROR,
    STOP,
} PATH_STATE;

//路径样式
typedef enum PATH_TYPE {
    BSPLINE = 30,
    STRAIGHT_LINE,
    TRACKBALL_LINE,
    SPECIAL_STRAIGHT_LINE,
    BESSEL_LINE,
} PATH_TYPE;

typedef struct PathParam {
    uint8_t pathType;
    uint8_t pathFinishDisp;
    uint8_t acceleratePercent;
    uint8_t descentPercent;
    uint8_t yawAdjustPercent;
    int32_t startSpeed;
    int32_t maxSpeed;
    int32_t endSpeed;
    int32_t decelerationDistance;
    Pose startPose;
    Pose endPose;
} PathParam;

typedef struct StraightLinePath {
    Pose startPose;
    Pose midPose;
    Pose endPose;
} StraightLinePath;

typedef struct B_Spline_PathCfg {
    float *knots;
    Pose *Control_Point;
    uint8_t k;
    uint8_t n;
} B_Spline_PathCfg;

typedef struct PATH_BesselControlPoint {
    Pose Path_BesselPoint_1; //起点
    point Path_BesselPoint_2; //控制点1
    point Path_BesselPoint_3; //控制点2
    Pose Path_BesselPoint_4; //终点
    point Path_BesselTransitionPoint[5]; //过度点
} PATH_BesselControlPoint;

//B样条曲线
typedef struct _Ni_k {
    float N[7][4];
} Ni_k;

typedef struct _BsplinePath {
    Pose ctrlPoint1;
    point ctrlPoint2;
    point ctrlPoint3;
    Pose ctrlPoint4;
} BsplinePath;


/************************************pid****************************************/
typedef struct PIDInfo {
    float target;
    float kp;
    float ki;
    float kd;
    float ti;
    float td;

    float ka;
    float kb;
    float kc;

    float err;
    float last_err;
    float lalast_err;

    float output;
    float last_output;

    float limit_Integral;
    float Integral;

    float MaxOutput;
    float T;
    float Un;

    bool pid_MODE; // 1 增量式 0 位置式
} PIDInfo;

typedef struct PoseRegulator {
    PIDInfo xRegulator;
    PIDInfo yRegulator;
    PIDInfo yawRegulator;
} PoseRegulator;

typedef enum PidCalMode {
    PID_OFF,
    PID_X_Y_ONLY,
    PID_YAW_ONLY,
    PID_X_Y_YAW,
} PidCalMode;

//平方
static inline float square(float x) {
    return (x * x);
}

//立方
static inline float cube(float x) {
    return (x * x * x);
}

//阶乘
static int Factorial(int n) {
    int ret = 1;
    for (int i = 1; i <= n; i++)
        ret *= i;
    return ret;
}

//排列组合A上标是n,下标是m
static int Amn(int m, int n) {
    int ret = 1;
    for (int i = 0; i < n; i++)
        ret *= (m - i);
    return ret;
}

//排列组合C上标是n,下标是m
inline static int Cmn(int m, int n) {
    return Amn(m, n) / Amn(n, n);
}

//角度制转弧度制
static inline float Angle2Radian(float Angle) {
    return (Angle * PI / 180.f);
}

//弧度制转角度制
static inline float Radian2Angle(float Radian) {
    return (Radian * 180.f / PI);
}

//精度限制在小数点后2位
static inline float To2DecimalPlaces(float src) {
    return (float) (int) (src * 100) / 100;
}

//精度限制在小数点后3位
static inline float To3DecimalPlaces(float src) {
    return (float) (int) (src * 1000) / 1000;
}

//限制最大值
static inline float Limit_Max(float value, float Max) {
    return (value > Max ? Max : value);
}

//限制最小值
static inline float Limit_Min(float value, float Min) {
    return (value < Min ? Min : value);
}

//同时限制最大值和最小值
static inline float Limit_MaxAndMin(float *value, float limit_Max, float limit_Min) {
    return Limit_Max(Limit_Min(*value, limit_Min), limit_Max);
}

// 将角度限制在 -180 ~ 180 之间
static inline float AngleLimit180(float dir) {
    return (fabsf(dir) > 180.f ? (dir - 360 * ((int32_t) (dir / 180) - (int32_t) (dir / 360))) : dir);
}

// 将角度限制在 -360 ~ 360 之间
static inline float AngleLimit360(float dir) {
    return (fabsf(dir) > 360.f ? dir - 360 * (int32_t) (dir / 360) : dir);
}

//将目标角度转化成与当前总角度同一周期的角度
static inline float Angle2SameCycle180(float srcCycledir, float destCycledir) {
    return (AngleLimit180(srcCycledir) + ((int) (destCycledir / 180) - (int) (destCycledir / 360)) * 360);
}

//将目标角度转化成与当前总角度同一周期的角度
static inline float Angle2SameCycle360(float srcCycledir, float destCycledir) {
    return (AngleLimit360(srcCycledir) + ((int) (destCycledir / 360)) * 360);
}

// 矢量合成
static inline vector vectorSynthesis(const vector *vel1, const vector *vel2) {
    vector SynthesisVel = {0};

    if (vel1 != NULL && vel2 != NULL) {
        float a = 0;
        float synthesisVelX = (vel1->size) * cosf(Angle2Radian(vel1->dir)) + (vel2->size) * cosf(
                                  Angle2Radian(vel2->dir));
        float synthesisVelY = (vel1->size) * sinf(Angle2Radian(vel1->dir)) + (vel2->size) * sinf(
                                  Angle2Radian(vel2->dir));


        SynthesisVel.size = sqrt(square(synthesisVelX) + square(synthesisVelY));

        a = atan2f(synthesisVelY, synthesisVelX);
        SynthesisVel.dir = Radian2Angle(a);
    }
    return SynthesisVel;
}

/*******************************************函数声明***************************************/
void Robot_Init();

/******************************************************************************************/

#endif //ENCODER_UPRE_INT_H
